#define ADXLREG_DATAX0       0x32
#define A_TO_READ (6)
#define ACC (0x53)
#define NUM 25

#include "wirish.h"
#include "i2c.h"


void setup()
{
  i2c_master_enable(I2C1, 0);  //Установить I2C1 интерфейс в хост-режиме  
  initAcc(); 
  initGyro();
  pinMode(BOARD_LED_PIN, OUTPUT);
  writeTo(ACC, 0x2C, 12);  //установить частоту обновления показаний акселерометра
//  SerialUSB.read();
  
}
int16 kk=0,kkmax;
int previousMillis = 0;
int time=0;


void loop()
{

  if (SerialUSB.available() > 0)
  {
        SerialUSB.read();
        kk=0;
        previousMillis=millis();
//        delay(20);
  };
  

  
  
//  SerialUSB.read();
//  for(int16 kk=0;kk<1000;kk++)
  {
  int16 acc[3];
  int16 dataX[NUM],dataY[NUM],dataZ[NUM];
  int16 bufX,bufY, bufZ;
  int16 sumx,sumy,sumz;
  int16 numX,numY,numZ;
  int16 Motorreg[6];
  int16 regAddress = ADXLREG_DATAX0;    //Адрес данных первой оси датчика ускорения ADXL345
  int16 bufacc;
  uint8 buff[A_TO_READ];
  uint8 bwrated[1];
  float sigmaX,sigmaY,sigmaZ,averX,averY,averZ;
  int i;
  
//  previousMillis=millis();
 
/*
  readFrom(ACC, 0x2C, 1, bwrated); //BW_RATE 
  SerialUSB.print(" BW_RATE ");
  SerialUSB.print(bwrated[0]);
  SerialUSB.print(" \n");
*/

  sumx=0;
  sumy=0;
//  sumz=0;

  for(i=0;i<NUM;i++)
  {
    readFrom(ACC, regAddress, A_TO_READ, buff); //Читайте данные датчика ускорения ADXL345
    dataX[i] = (((int16)buff[1]) << 8) | buff[0] + 0; 
    dataY[i] = (((int16)buff[3]) << 8) | buff[2] + 0;
//    dataZ[i] = ((((int16)buff[5]) << 8) | buff[4]) + 0;
    sumx=sumx+dataX[i];
    sumy=sumy+dataY[i];
//    sumz=sumz+dataZ[i];
  };
  averX=1.0*sumx/NUM;
  averY=1.0*sumy/NUM;
//  averZ=1.0*sumz/(NUM);
  //посчитать срднеквадратичное отклонение
  
  sigmaX=0;
  sigmaY=0;  
//  sigmaZ=0;
  for(int i=0;i<NUM;i++)
  {
    sigmaX=sigmaX+pow(((1.0*sumx/NUM)-dataX[i]*1.0),2);
    sigmaY=sigmaY+pow(((1.0*sumy/NUM)-dataY[i]*1.0),2);
//    sigmaZ=sigmaZ+pow(((1.0*sumz/NUM)-dataZ[i]*1.0),2);
  };
//  SerialUSB.print(" \n");
  sigmaX=pow(sigmaX/(NUM*(NUM-1)),0.5);
  sigmaY=pow(sigmaY/(NUM*(NUM-1)),0.5);
//  sigmaZ=pow(sigmaZ/(NUM*(NUM-1)),0.5);
  
  for(int i=0;i<NUM;i++)
  {
    if( (dataX[i]<(averX-3*sigmaX)) | (dataX[i]>(averX+3*sigmaX)) )
      {dataX[i]=555;}
    if( (dataY[i]<(averY-3*sigmaY)) | (dataY[i]>(averY+3*sigmaY)) )
      {dataY[i]=555;}
//    if( (dataZ[i]<(averZ-3*sigmaZ)) | (dataZ[i]>(averZ+3*sigmaZ)) )
//      {dataZ[i]=555;}
  };
//пересчитать среднее

  sumx=0;
  sumy=0;
//  sumz=0;  
  numX=0;
  numY=0;
//  numZ=0;
  for(int i=0;i<NUM;i++)
  {
    if(dataX[i]!=555){numX++;sumx=sumx+dataX[i];};
    if(dataY[i]!=555){numY++;sumy=sumy+dataY[i];};
//    if(dataZ[i]!=555){numZ++;sumz=sumz+dataZ[i];};
  };
  averX=1.0*sumx/numX;
  averY=1.0*sumy/numY;
//  averZ=sumz/numZ;


// вывод данных--------------------------------

/*

  SerialUSB.print("dataX ");  
  for(int i=0;i<NUM;i++)
  {
    if( (dataX[i]<(averX-3*sigmaX)) | (dataX[i]>(averX+3*sigmaX)) )
      {
        SerialUSB.print(" a");
        SerialUSB.print(dataX[i]);
        SerialUSB.print(" ");
      }
      else 
      {
        SerialUSB.print(dataX[i]);
        SerialUSB.print(" ");
      }
  };
  SerialUSB.print("sigmaX ");
  SerialUSB.print(sigmaX);
  SerialUSB.print(" averX ");
  SerialUSB.print(averX);
  SerialUSB.print(" \n");
  
  SerialUSB.print("dataY ");  
  for(int i=0;i<NUM;i++)
  {
    SerialUSB.print(dataY[i]);
    SerialUSB.print(" ");
  };
  SerialUSB.print("sigmaY ");
  SerialUSB.print(sigmaY);
  SerialUSB.print(" averY ");
  SerialUSB.print(averY);
  SerialUSB.print("\n");


//  SerialUSB.print(bufz);
//  SerialUSB.print(" ");

*/



//previousMillis = millis()-previousMillis;
  time = millis()-previousMillis;
  SerialUSB.print(kk);
  SerialUSB.print(" ");
  SerialUSB.print(time);
  SerialUSB.print(" ");
  SerialUSB.print(averX);
  SerialUSB.print(" ");  
  SerialUSB.print(averY);
//  SerialUSB.print(" ");
//  SerialUSB.print(averZ);




  kk++;
  if(kk==1000)
   {
     kk=0;
//     kkmax=SerialUSB.read();
   };
  SerialUSB.print("\n");
  toggleLED();
  };//end bigi
//  delay(100);  
}
